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Abstract — Orientation  of  a  static  or  slow-moving  rigid  body 
can  be  determined  from  the  measured  gravity  and  local  magnetic 
field  vectors.  Some  formulation  of  the  Quaternion  ESTimator 
(QUEST)  algorithm  is  commonly  used  to  solve  this  problem. 
Triads  of  accelerometers  and  magnetometers  are  used  to  measure 
gravity  and  local  magnetic  field  vectors  in  sensor  coordinates.  In 
the  QUEST  algorithm,  local  magnetic  field  measurements  affect 
not  only  the  estimation  of  yaw  but  also  that  of  roll  and  pitch.  Due  to 
the  deviations  in  the  direction  of  the  magnetic  field  vector  between 
locations,  it  is  not  desirable  to  use  magnetic  data  in  calculations 
that  are  related  to  the  determination  of  roll  and  pitch.  This  paper 
presents  a  geometrically  intuitive  3-degree-of-freedom  (3-DOF) 
orientation  estimation  algorithm  with  physical  meaning  [which  is 
called  the  factored  quaternion  algorithm  (FQA)],  which  restricts 
the  use  of  magnetic  data  to  the  determination  of  the  rotation 
about  the  vertical  axis.  The  algorithm  produces  a  quaternion 
output  to  represent  the  orientation.  Through  a  derivation  based 
on  half-angle  formulas  and  due  to  the  use  of  quaternions,  the  com¬ 
putational  cost  of  evaluating  trigonometric  functions  is  avoided. 
Experimental  results  demonstrate  that  the  proposed  algorithm 
has  an  overall  accuracy  that  is  essentially  identical  to  that  of  the 
QUEST  algorithm  and  is  computationally  more  efficient.  Addi¬ 
tionally,  magnetic  variations  cause  only  azimuth  errors  in  FQA 
attitude  estimation.  A  singularity  avoidance  method  is  introduced, 
which  allows  the  algorithm  to  track  through  all  orientations. 

Index  Terms — Accelerometers,  inertial  sensors,  magnetic  sen¬ 
sors,  motion  measurement,  orientation  estimation,  quaternions. 

I.  Introduction 

ACCURATE  real-time  tracking  of  the  orientation  or 
attitude  of  rigid  bodies  has  applications  in  robotics, 
aerospace,  underwater  vehicles,  synthetic  reality,  etc.  For  syn¬ 
thetic  reality  applications,  the  human  body  can  be  viewed  as 
an  articulated  rigid  body  consisting  of  approximately  15  links. 
If  the  orientation  relative  to  a  fixed  reference  frame  can  be 
determined  for  each  of  the  links,  then  the  overall  posture  of  the 
human  subject  can  accurately  be  rendered  and  communicated 
in  real  time.  The  orientation  of  a  static  or  slow-moving  indi¬ 
vidual  limb  segment  can  be  measured  through  the  attachment 
of  an  inertial/magnetic  sensor  module.  Such  sensor  modules 
typically  contain  a  triad  of  orthogonally  mounted  accelerom- 
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eters  and  a  triad  of  orthogonally  mounted  magnetometers.  The 
accelerometers  are  used  to  measure  the  gravity  vector  that  is 
relative  to  the  coordinate  frame  of  the  sensor  module.  The 
magnetometers  serve  a  similar  function  for  the  local  magnetic 
field  vector.  Since  accelerometers  actually  sense  the  sum  of 
gravity  and  linear  acceleration  due  to  motion,  low-pass  filter¬ 
ing  is  generally  required  to  discriminate  against  the  latter.  In 
dynamic  applications  such  as  rapid  human  movement,  a  triad 
of  angular  rate  sensors  is  usually  added  as  a  “complementary” 
high-frequency  source  of  orientation  information.  In  any  case, 
such  approaches  to  orientation  estimation  are  dependent  only 
on  passive  measurement  of  physical  quantities  that  are  directly 
related  to  the  rate  of  rotation  and  orientation  of  a  rigid  body. 
Since  no  generated  signals  are  involved,  there  are  no  restrictions 
on  the  range  of  operation.  All  latency  in  such  a  system  is  due 
to  the  computational  demands  of  the  data  processing  algorithms 
and  not  due  to  the  physical  characteristics  of  a  generated  source. 

Extensive  research  has  been  conducted  to  investigate  full 
3-degree-of-freedom  (3-DOF)  orientation  tracking  using 
inertial/magnetic  sensor  modules.  Foxlin  et  al.  [1],  [2]  describe 
two  commercial  nine-axis  sensing  systems  designed  for  head¬ 
tracking  applications.  Bachman  et  al.  proposed  a  quaternion- 
based  complementary  filter  for  human-body-motion  tracking. 
The  filter  is  able  to  track  through  all  orientations  without 
singularities  and  continuously  correct  for  drift  without  a 
need  for  stationary  periods  using  nine-axis  inertial/magnetic 
sensor  module  data  [3],  [4].  Gallagher  et  al.  presents  a  simpler 
complementary  filter  algorithm  with  lower  computational 
complexity  in  [5].  Luinge  describes  a  Kalman  filter  that  is 
designed  for  human-body-tracking  applications.  The  filter  is 
based  on  the  use  of  accelerometers  and  rate  sensors.  The  drift 
about  the  vertical  axis  is  reduced  by  limiting  body  segment 
orientation  using  a  kinematic  human  body  model  [6].  Rather 
than  estimating  individual  limb  segment  orientations  relative 
to  a  fixed  reference  frame,  Zhu  and  Zhou  [7]  determine  joint 
angles  in  axis/angle  form  using  the  data  from  the  two  nine-axis 
sensors  that  are  mounted  on  the  inboard  and  outboard  sides 
of  the  joint.  Yan  and  Yuan  [8]  describe  an  orientation-tracking 
algorithm  that  uses  low-cost  sensor  modules  to  take  two-axes 
measurements  of  gravity  and  the  local  magnetic  field.  In  a 
manner  that  is  similar  to  the  method  described  in  this  paper, 
elevation,  roll,  and  azimuth  angles  are  sequentially  calculated. 
The  angles  are  used  to  construct  rotation  matrices,  and  the 
use  of  trigonometric  functions  is  required.  The  method  is 
limited  to  orientation  tracking  within  a  hemisphere.  In  [9], 
Gebre-Egziabher  et  al.  describe  an  attitude-determination 
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algorithm  for  aircraft  applications.  The  algorithm  is  based 
on  a  quaternion  formulation  of  Wahba’s  problem  [10],  where 
magnetometer  and  accelerometer  measurements  are  used  to 
determine  attitude. 

The  TRIAD  algorithm  [11]  is  a  single  frame  deterministic 
method  for  solving  Wahba’s  problem.  It  requires  normalized 
measurements  of  two  nonparallel  reference  vectors  as  input. 
It  produces  a  suboptimal  orientation  estimate  in  the  form  of 
a  3  x  3  rotation  matrix.  The  algorithm  constructs  two  triads 
of  orthonomal  unit  vectors.  The  two  triads  are  the  components 
of  an  inertial  frame  expressed  in  both  the  body  and  Earth- 
fixed  reference  frames.  Let  ba  and  bm  be  the  normalized  ac¬ 
celerometer  and  magnetometer  measurements  that  are  relative 
to  the  body  frame  of  the  gravity  and  magnetic  field  reference 
vectors  ( Eg  and  Em).  The  reference  vectors  are  expressed 
relative  to  an  Earth-fixed  frame,  and  like  the  measurement 
vectors,  they  are  normalized  to  unit  length.  The  first  triad  is 
given  by 


si 


E 


9 


S  2 


( Eg  xE  m) 
| Eg  xE  m\ 


S3  =  Sl  X  §2- 


(1) 

(2) 

(3) 


The  second  triad  is  given  by 

fi  =ba 

„  ( ba  xb  m) 
r2  | ba  xb  m\ 

f3  =fi  x  f2. 


(4) 

(5) 

(6) 


These  triads  are  then  used  to  create  measurement  and  reference 
matrices 


M, „ea  =  [f\  f2  f3],  Mre f  =  [sX  S2  S3].  (7) 

The  orientation  matrix  A  representing  the  attitude  of  a  rigid 
body  is  then  simply 

A  =  Mmea  MEf.  (8) 

If  the  measurements  of  the  gravity  and  the  magnetic  field  are 
ordered  as  described  earlier,  the  cross-products  that  are  used  to 
calculate  s2  and  fi2  eliminate  any  contribution  of  the  magnetic 
measurements  relative  to  the  vertical  axis.  Thus,  pitch  and 
roll  components  of  orientation  are  determined  using  only  the 
accelerometer  measurements. 

The  QUaternion  ESTimator  (QUEST)  algorithm  is  a  popular 
algorithm  for  single-frame  estimation  of  a  quaternion  that  rep¬ 
resents  the  attitude  of  a  rigid  body  relative  to  a  fixed  coordinate 
system.  The  algorithm  was  created  to  solve  Wahba’s  problem 
[10]  in  the  context  of  spacecraft  attitude  determination.  Given  a 
set  of  3-D  known  reference  unit  vectors  V\,  V2, . . . ,  Vn  and  a  set 
of  the  corresponding  observation  or  measurement  unit  vectors 
W\ ,  W2 , .  •  • ,  Wn  (which  could  be  the  direction  of  the  sun  or  a 
star  observed  from  a  spacecraft  measured  in  the  spacecraft’s 
body  frame),  Wahba’s  problem  is  to  find  the  least  squares 


estimate  of  spacecraft  attitude  by  minimizing  the  following  loss 
function: 


L(A)  =  ^J2ai(Wi-AVi)T(Wi-AVi)  (9) 

i=l 

with  respect  to  the  3x3  orthogonal  orientation  matrix  A,  where 
Oi,  o2, . . . ,  an  are  nonnegative  weighting  coefficients.  The  min¬ 
imum  number  of  measurement  and  reference  vector  pairs  is 
two.  Early  solutions  to  Wahba’s  problem  directly  computed  the 
orientation  matrix  A  [12].  Davenport  [13]  introduced  a  method 
of  parameterizing  the  orientation  matrix  by  a  unit  quaternion  q 
and  proved  that  the  loss  function  (9)  can  be  transformed  into  a 
quadratic  gain  function  of  the  unit  quaternion  in  the  form  of 

n 

G(A(q))  =  ^2  a,  -  L(A(q))  =  qT Kq  (10) 

i=l 

where  K  is  a  4  x  4  matrix  constructed  from  the  reference 
vectors  Vt,  measurement  vectors  IT) ,  and  weighting  coefficients 
Oj,  i  =  1,  2, . . . ,  n.  Based  on  Davenport’s  work,  Shuster  and 
Oh  derived  the  QUEST  algorithm  [14]  and  showed  that  the 
optimal  quaternion  q  that  maximizes  the  gain  function  (10) 
while  satisfying  the  unit  quaternion  (unit  norm)  constraint  is 
the  eigenvector  of  the  K  matrix  corresponding  to  the  largest 
eigenvalue  of  K.  Thus,  the  problem  is  reduced  to  finding  the 
eigenvalues  and  eigenvectors  of  a  4  x  4  matrix. 

In  body-tracking  applications  based  on  the  use  of  small 
inertial/magnetic  sensors  [4],  the  gravity  and  local  magnetic 
field  vectors  are  often  measured  and  compared  to  reference 
vectors  in  order  to  determine  orientation.  In  the  case  of  the 
gravity  vector,  the  assumption  that  it  is  fixed  leads  to  no 
difficulties  since  this  vector  points  straight  down  in  any  inertial 
frame  located  on  or  near  the  surface  of  the  Earth.  Making  the 
same  assumption  regarding  the  local  magnetic  field  vector  can 
lead  to  problems.  In  a  typical  room  setting,  the  direction  of 
the  local  magnetic  field  vector  can  be  expected  to  vary  due 
to  the  presence  of  ferrous  objects  or  electrical  appliances.  In 
inertial/magnetic-tracking  algorithms,  the  local  magnetic  field 
vector  is  commonly  treated  as  a  fixed  reference.  It  is  assumed 
that  this  reference  will  remain  constant.  If  it  does  not,  algo¬ 
rithms  such  as  QUEST  will  be  prone  to  errors  not  only  in 
azimuth  but  also  in  pitch  and  roll. 

This  paper  presents  an  alternative  algorithm  for  estimating 
orientation  based  on  a  set  of  measurements  from  triads  of 
orthogonally  mounted  magnetometers  and  accelerometers.  It 
is  called  the  factored  quaternion  algorithm  (FQA).  It  is  an 
intuitive  alternative  to  the  TRIAD  and  QUEST  algorithms  that 
yields  certain  advantages.  In  the  FQA,  local  magnetic  field  data 
are  used  only  in  azimuth  angle  calculations.  This  decoupling  of 
accelerometer  and  magnetometer  data  eliminates  the  influence 
of  magnetic  variations  on  calculations  that  determine  pitch 
and  roll.  Through  a  derivation  based  on  half-angle  formulas, 
the  computational  cost  of  computing  trigonometric  functions 
is  avoided.  The  algorithm  produces  a  quaternion  output.  It 
is  able  to  track  through  all  orientations  without  singularities. 
The  FQA  and  the  TRIAD  algorithm  produce  an  equivalent 
solution  to  the  same  problem,  with  the  exception  that  the  former 
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produces  a  quaternion,  and  the  latter  produces  a  rotation  matrix. 
Experimental  results  in  which  the  FQA  is  compared  with  the 
QUEST  algorithm  indicate  that  it  has  nearly  identical  accuracy 
at  a  comparable  or  lower  computational  expense.  The  QUEST 
algorithm  was  chosen  as  a  benchmark  for  comparison  because 
it  is  an  optimal  algorithm  and  produces  orientation  estimates  in 
quaternion  form,  as  does  the  FQA. 

The  primary  contributions  of  this  paper  are  the  following: 

1)  derivation  of  a  new  geometrically  intuitive  algorithm  for 
determining  orientation  that  is  relative  to  an  Earth-fixed 
reference  frame  based  on  a  set  of  accelerometer  and 
magnetometer  measurements; 

2)  a  singularity  avoidance  method  that  allows  the  algorithm 
to  track  through  all  orientations; 

3)  experimental  results  which  validate  the  performance  of 
the  algorithm. 

The  rest  of  this  paper  is  organized  as  follows.  Section  II 
presents  the  derivation  of  the  FQA.  Section  III  describes  the 
experiments  in  which  the  factored  algorithm  is  compared  to 
the  QUEST  algorithm  for  efficiency  and  accuracy.  The  abil¬ 
ity  of  the  algorithm  to  track  through  all  orientations  without 
singularities  is  demonstrated  as  is  its  decoupling  property.  The 
final  section  discusses  the  experimental  results  and  provides  a 
summary. 

II.  Factored  Quaternion  Algorithm  (FQA) 

The  FQA  presented  in  this  section  is  for  estimating  the 
orientation  of  a  static  or  slow-moving  rigid  body  based  on  Earth 
gravity  and  magnetic  field  measurements  [15].  This  algorithm 
is  not  applicable  to  situations  in  which  relatively  large  linear 
accelerations  due  to  dynamic  motion  are  present,  unless  it  is 
used  in  a  complementary  or  optimal  filter  together  with  angular 
rate  information.  Sensor  modules  such  as  MARG  III,  which  is 
described  in  [16],  contain  a  triad  of  accelerometers,  a  triad  of 
magnetometers,  and  a  triad  of  angular  rate  sensors,  and  it  can 
be  used  to  provide  measurement  data  for  the  FQA. 

In  a  typical  application,  a  sensor  module  is  employed  as  a 
strap  down  inertial  measurement  unit  (IMU)  that  is  attached 
to  a  rigid  body  whose  orientation  is  to  be  determined.  To 
facilitate  the  analysis,  it  is  convenient  to  define  three  coordinate 
systems.  An  Earth-fixed  coordinate  system  xeyeze  is  defined  to 
follow  the  North-East-down  (NED)  convention,  i.e.,  x,  points 
North,  ye  points  East,  and  ze  points  down.  A  body  coordinate 
system  XbybZb  is  attached  to  the  rigid  body  whose  orientation 
is  to  be  measured.  The  sensor  module  has  its  own  coordinate 
system  xsyszs  corresponding  to  the  axes  of  three  orthogonally 
mounted  accelerometers/magnetometers.  Since  the  sensor  mod¬ 
ule  is  rigidly  attached  to  the  rigid  body,  the  body  coordinate 
system  XbybZb  differs  from  the  sensor  coordinate  system  xsyszs 
by  a  constant  offset.  For  the  convenience  of  discussions,  in  what 
follows,  the  body  coordinate  system  is  assumed  to  coincide 
with  the  sensor  coordinate  system. 

A.  Quaternion  Rotation  Operator 

According  to  Euler’s  theorem  on  finite  rotations,  an  ar¬ 
bitrary  sequence  of  rotations  in  space  can  always  be  de¬ 


scribed  by  a  rotation  about  a  certain  axis  through  a  specified 
angle.  Let 


u  1 


U  2 


U3 


(ID 


be  the  unit  vector  in  3-D  space  that  represents  the  rotation 
axis,  and  let  (3  be  the  rotation  angle.  The  Euler  parameters  are 
defined  by 


q0  =  cos 


(12) 


wi 

u2 

U3 


(13) 


Because  an  arbitrary  rotation  may  be  described  by  three  inde¬ 
pendent  parameters,  the  four  Euler  parameters  are  constrained 
to  satisfy  the  relation  [17] 


9o  +  9i  +  92  +ql  =  1.  (14) 

These  four  parameters  are  also  referred  to  as  a  unit  quaternion, 
which  is  commonly  written  in  the  form 

q  =  (go  gi  92  93)  (15) 

where  go  is  the  scalar  (or  real)  part,  and  [gi  <72  93] T  is  the 
vector  part.  Unit  quaternions  can  be  used  to  perform  rotation 
operations  in  3-D  space  [18].  Specifically,  for  any  vector  v  = 
[i>i  v2  V3\t  in  3-D  space,  the  following  operation  produces  a 
vector  v1  by  rotating  the  vector  v  about  the  axis  that  is  defined 
by  u  through  an  angle  (3: 


v'  =  qvq  1  (16) 

In  this  expression,  all  multiplications  are  quaternion  multiplica¬ 
tions,  v  and  v'  are  treated  as  pure  vector  quaternions  whose  real 
part  is  zero,  and  q  1  is  the  inverse  quaternion  of  q  [18], 


B.  Elevation  Quaternion 

A  rigid  body  is  said  to  be  in  its  reference  orientation  when 
its  XbybZb- axes  are  aligned  with  those  of  the  Earth  coordinate 
system.  It  is  known  that  a  rigid  body  can  be  placed  in  an 
arbitrary  orientation  by  first  rotating  it  about  its  2-axis  by  an 
angle  ft  (azimuth  or  yaw  rotation),  then  about  its  y- axis  by 
angle  9  (elevation  or  pitch  rotation),  and  finally  about  its  x-axis 
by  angle  <f>  (bank  or  roll  rotation). 

In  order  to  derive  a  quaternion  describing  only  elevation,  it  is 
useful  to  note  that,  when  a  rigid  body  is  moving  at  a  constant 
velocity  and  is  in  a  fixed  orientation,  an  accelerometer  measures 
only  gravity.  Furthermore,  the  x-axis  accelerometer  senses  only 
the  component  of  gravity  along  the  a’-axis,  and  this  component, 
in  turn,  depends  only  on  the  elevation  angle.  This  can  be  seen 
from  the  following  argument.  Starting  with  the  rigid  body  in  its 
reference  orientation,  the  a; -axis  accelerometer  is  perpendicular 
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to  gravity  and  thus  registers  zero  acceleration.  The  y- axis 
accelerometer  also  reads  zero,  while  the  2-axis  accelerometer 
reads  — g .  If  the  body  is  then  rotated  in  azimuth  about  its 
2-axis,  the  x-axis  accelerometer  still  reads  zero,  regardless  of 
the  azimuth  angle.  If  the  rigid  body  is  next  pitched  up  through 
an  angle  9,  the  x-axis  accelerometer  will  read 

ax=g  sin  9  (17) 

and  the  2-axis  accelerometer  will  read 

az  =  —g  cos  9  (18) 

where  g  =  9.81  m/s2  is  the  gravitational  acceleration,  and 


Elevation  is  a  rotation  about  the  y-axis.  The  unit  quaternion 
representing  elevation  can  now  be  computed  using  (12)  and 
(13),  and  values  for  the  half-angle  trigonometric  functions  are 
as  follows: 

0  9 

qe  =  cos  -(1  0  0  0)+  sin  -(0  0  1  0).  (25) 

C.  Roll  Quaternion 

The  acceleration  measured  by  the  2-axis  accelerometer  with 
roll  angle  </>  =  0  is  given  by  (18).  Changing  the  azimuth  angle 
does  not  alter  this  measurement,  but  changing  roll  does.  A  more 
general  formula  for  y-axis  accelerometer  reading  is 


ax 

av 

az 


(19) 


dy  =  —g  cos  9  sin  </>. 


Likewise,  the  2-axis  accelerometer  will  read 


(26) 


is  the  measured  acceleration  vector  in  the  body  coordinate 
system.  For  convenience,  the  accelerometer  and  magnetometer 
outputs  from  a  sensor  module  are  normalized  to  unit  vec¬ 
tors.  Let  a  denote  the  normalized  vector  of  the  acceleration 
measurements 


az  =  —  y  cos  0  cos  </>.  (27) 

In  terms  of  the  normalized  acceleration  measurement,  the  ear¬ 
lier  two  equations  can  be  written  as 


a 


dy 

az 


(20) 


where  |a|  is  the  norm  of  the  acceleration  vector  a.  It  follows 
from  (17)  that  the  value  for  sin  9  can  be  expressed  as 


ay  =  —  cos  9  sin  (j>  (28) 

dz  =  —  cos  9  cos  (j>  (29) 

where  the  value  for  cos  9  is  determined  by  (22).  If  cos  9  is 
not  equal  to  zero,  the  values  of  sin  4>  and  cos  o  can  easily  be 
determined  by 


sin#  =  ax.  (21) 

The  value  for  cos  9  can  be  computed  from 

cos  9  =  sjl  —  sin2  9.  (22) 

It  should  be  noted  that  a  positive  value  for  cos  9  is  assumed  in 
the  preceding  equation.  This  is  because  the  elevation  angle  9  is, 
by  convention,  restricted  to  the  range  of  — 7r/2  <  9  <  7t/2,  and 
cos  9  is  positive  over  this  entire  range.  In  addition,  if  the  rigid 
body  is  rolled  about  its  x-axis,  (18)  will  change,  but  (17)  will 
remain  the  same.  This  means  that  (17)  holds  for  any  orientation 
of  the  rigid  body. 

In  order  to  obtain  an  elevation  quaternion  using  (12)  and  (13), 
a  value  is  needed  for  sin(#/2)  and  cos(0/2).  From  trigonomet¬ 
ric  half-angle  formulas,  half-angle  values  are  given  by 


sign(sin  9)\/{  1  —  cos  9) /2 

(23) 

\/(l  +  cosd)/2 

(24) 

where  sign()  is  the  sign  function  that  returns  +1  for  positive 
arguments  and  —1  for  negative  arguments.  The  sign  function  is 
not  needed  in  (24)  since  cos (9/2)  is  always  positive  within  the 
elevation  angle  range. 


sin  <j>  = 

—  dy/  cos  9 

(30) 

cosy)  = 

—  az/cos  9. 

(31) 

If  cos  9  is  equal  to  zero,  it  means  that  x-axis  of  the  body 
coordinates  is  vertically  oriented.  In  such  cases,  the  roll  angle 
is  undefined,  and  it  can  be  assumed  to  have  a  value  that  is 
equal  to  zero.  The  range  of  the  roll  angle  <p  is  by  convention 
restricted  to  —  n  <  <j>  <  ir.  Thus,  the  half-angle  values  for  <f> 
can  be  computed  in  a  manner  that  is  similar  to  (23)  and  (24), 
with  one  exception.  When  cos  <p  =  —  1  and  sin  <fi  =  0,  the  use 
of  (23)  and  (24)  will  result  in  a  value  of  zero  for  both  sin(</>/2) 
and  cos(</>/2).  This  case  can  be  treated  in  implementation  by 
assigning  a  value  of  one  to  the  sign  function  when  its  argu¬ 
ment  is  zero.  Having  obtained  the  half-angle  sine  and  cosine 
values  for  the  roll  angle  </>,  the  roll  quaternion  is  computed  as 
follows: 

qr  =  cos  ^(1  0  0  0)  + sin  ^(0  1  0  0).  (32) 

D.  Azimuth  Quaternion 

Since  azimuth  rotation  has  no  effect  on  the  estimation  of 
roll  or  elevation  quaternions  from  the  accelerometer  data,  the 
strategy  employed  in  this  paper  for  azimuth  quaternion  esti¬ 
mation  is  to  first  solve  for  the  elevation  and  roll  quaternions. 
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These  can  then  be  used  to  rotate  the  normalized  magnetic  field 
measurement  vector  in  the  body  coordinate  system 


b 


m  = 


bmx 

bmv 

bmz 


(33) 


into  an  intermediate  Earth  coordinate  system  by  the  quaternion 
rotation  operation 


em  =  qeqrbmqr1qe1.  (34) 

In  this  expression,  bm  stands  for  the  pure  vector  quaternion  of 
the  3-D  vector  itself,  i.e.,  bm  =  (0  bmx  brny  bmz).  The  same 
convention  is  used  for  em  .  In  the  absence  of  measurement  error, 
em  should  agree  with  the  known  local  normalized  magnetic 
field  vector  n  =  [nx  ny  nz]T,  except  for  the  effects  of  azimuth 
rotation  on  the  sensor  magnetometer  readings.  In  such  a  case, 
nz  =  emz,  and 

nx 
_ny 

where  ip  is  the  azimuth  angle.  Before  proceeding  further,  it 
should  be  noted  that  (35)  implies  that  the  two  2-D  vectors  differ 
only  in  orientation.  In  fact,  the  experimental  data  show  that,  in 
the  presence  of  magnetic  interference  and  measurement  noise, 
they  may  also  differ  in  length.  To  compensate  for  this  effect,  the 
vectors  on  both  sides  of  (35)  can  be  normalized.  Specifically, 
let  the  normalized  local  magnetic  field  reference  vector  in  the 
horizontal  plane  be 


cos  ip  —  sin  ip 
sin  ip  cos  ip 


emx 

(35) 


Having  obtained  all  three  rotation  quaternions,  the  quater¬ 
nion  estimate  representing  the  orientation  of  the  rigid  body  is 
finally  given  by 

q=  qaqe  qr.  (41) 


E.  Singularity  Avoidance  in  Implementation 


The  FQA  presented  earlier  takes  the  normalized  acceleration 
measurement  vector  and  the  normalized  local  magnetic  field 
measurement  vector  as  its  inputs,  and  it  produces  a  quaternion, 
which  is  its  output.  It  is  a  single-frame  algorithm,  i.e,,  it  takes 
measurements  at  a  single  instant  of  time  and  produces  an 
output.  It  does  not  require  a  history  of  measurements  at  multiple 
instants  of  time. 

From  the  two  measurement  vectors,  the  half-angle  values  of 
sine  and  cosine  for  each  rotation  angle  are  first  computed.  Then, 
the  corresponding  quaternion  for  each  rotation  is  computed. 
Finally,  the  overall  orientation  quaternion  is  computed  by  (41). 
It  should  be  emphasized  that  the  algorithm  does  not  evaluate 
trigonometic  functions  at  any  step. 

Although  quaternions,  when  used  to  represent  the  3-D  ori¬ 
entation,  do  not  have  singularities,  the  FQA  described  earlier 
uses  three  angles  to  derive  the  quaternion  estimate.  It  is  known 
that  any  three-parameter  representation  of  3-D  orientation  is 
inevitably  singular  at  some  point  [19].  Without  exception,  the 
FQA  has  a  singularity,  as  does  the  QUEST  algorithm.  The 
QUEST  algorithm  uses  the  Gibbs  vector 


P 


1 

qo 


(42) 


N  = 


Nx 

Ny. 


1 


nx 

ny 


(36) 


and  the  corresponding  quantity  measured  by  the  magne¬ 
tometer  be 


~MX 

1 

emx 

[My  \ 

i  lerni  ±e  ml 

_emy_ 

(37) 


With  these  definitions,  (35)  becomes 

'Nx 

Ny 


cosip 

—  sin  ip 

Mx 

sin  ip 

cos  ip 

My 

(38) 


from  which  the  value  of  cos  ip  and  sin  ip  can  be  solved  as 


cosip 

sin  ip 

Mx 

~My 


My 

Mx 


ixi 

(39) 


The  azimuth  angle  ip  is  restricted  to  the  range  —  7r  <  ip  <  n. 
The  half-angle  formulas  given  by  (23)  and  (24)  can  again  be 
used  to  compute  the  half-angle  sine  and  cosine  values  for  ip. 
The  azimuth  quaternion  is  then  given  by 

qa  =  cos  ^(1  0  0  0)+  sin  ^(000  1).  (40) 


in  its  derivation  and  is  at  a  singular  point  if  qg  =  0.  A  method 
that  is  similar  to  the  method  of  sequential  rotations  discussed  in 
[20]  is  described  below  to  avoid  singularities  in  the  numerical 
implementation.  A  singularity  occurs  in  the  FQA  if  the  eleva¬ 
tion  angle  is  ±90°.  This  happens  when  cos#  =  0  in  (30)  and 
(31).  In  implementation,  the  first  step  of  the  algorithm  is  to 
check  the  value  of  cos  9.  If  the  value  of  cos  9  is  smaller  than  a 
predefined  constant  e  (e.g.,  e  =  0.1),  the  procedures  described 
below  are  implemented  to  circumvent  the  numeric  difficulty  of 
having  a  small  number  in  the  denominator. 

If  cos  9  <  e,  the  elevation  angle  is  close  to  ±90°.  To  deal  with 
this  situation,  the  normalized  acceleration  measurement  vector 
a  and  normalized  magnetic  field  measurement  vector  brn  in  the 
body  frame  are  rotated  about  the  body  coordinate  yy- axis  by  an 
angle  a  to  obtain  the  following  offset  (rotated)  measurement 
vectors: 

^offset  qa  c  qa  (43) 

tr+fFset  da  tn  qa  (44) 

where  qa  is  the  offset  (rotation)  quaternion  given  by 

qa  =  cos  ^(1  0  0  0)  ±  sin  ^(00  10).  (45) 

Under  the  condition  of  cos  9  <  e,  the  offset  measurement  vec¬ 
tors  will  be  used  in  place  of  the  original  measurement  vectors 
to  carry  out  the  FQA.  The  resultant  orientation  quaternion 
estimate  from  (41),  in  this  case,  is  called  qa\t. 
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Fig.  1.  Roll,  pitch,  and  yaw  angles;  singularity  condition;  and  switch  flag  during  a  180°  rotation  about  the  pitch  axis  with  ideal  simulated  data. 


The  value  of  a  can  arbitrarily  be  chosen  as  long  as  it  is 
sufficiently  far  away  from  zero.  It  is  chosen  to  be  20°  in  this 
discussion.  Rotating  measurement  vectors  about  y^-ax is  by  20° 
is  equivalent  to  rotating  the  (original)  body  coordinate  system 
XbVbZb  to  a  temporarily  offset  body  coordinate  system  x'by'bz'b 
about  the  y^-axis  by  —20°.  The  symbol  qa\t  represents  the  ori¬ 
entation  of  x’bybzb  in  the  Earth  coordinate  system.  The  quater¬ 
nion  estimate  q  representing  the  orientation  of  the  original  body 
coordinate  system  XbybZb  is  given  by  the  following  compound 
quaternion  (i.e.,  rotating  x'by'bzb  back  to  XbybZb  about  the 
y'b- axis  by  20°): 

q  =  gait  qa  ■  (46) 

To  demonstrate  how  the  singularity  avoidance  method  works, 
ideal  measurements  as  well  as  noisy  measurements  for  a  180° 
rotation  about  the  pitch  axis  were  synthetically  generated. 
Fig.  1  shows  the  results  with  ideal  measurements.  The  top 
three  plots  are  trajectories  of  roll,  pitch,  and  yaw  angles.  The 
bottom  two  plots  depict  the  value  of  cos  9  and  the  switch  flag. 
The  value  of  cos  9  is  an  indication  of  the  singularity  condition, 
and  the  switch  flag  indicates  when  the  singularity  avoidance 
method  is  invoked.  As  expected,  the  pitch  angle  increases  from 
0°  to  90°,  while  the  roll  and  yaw  angles  remain  at  zero  during 
the  first  half  period.  As  the  pitch  angle  approaches  90°,  the 
value  of  cos  9  drops  nearly  to  zero.  When  cos  9  is  less  than  e 
(whose  value  is  chosen  as  0.1  in  this  testing),  the  singularity 
avoidance  method  is  activated  during  the  period  of  sample 
numbers  from  about  820  to  980,  as  shown  in  Fig.  1.  During 
this  period,  the  value  of  cos  9  is  lifted  upwards  to  be  away  from 
zero.  The  value  of  the  offset  angle  a  is  chosen  to  be  20°. 

Owing  to  the  conventional  choice,  the  pitch  angle  is  limited 
from  —90°  to  90°.  As  a  result,  the  orientation  of  95°  pitch. 


0°  roll,  and  0°  yaw  is  depicted  as  85°  pitch,  180°  roll,  and 
180°  yaw  in  Fig.  1.  This  is  the  reason  why  the  pitch  angle 
increases  from  0°  to  90°  and  then  decreases  from  90°  to  0°, 
while  in  the  actual  rotation,  it  increases  from  0°  to  180°. 

Fig.  2  shows  the  results  with  noisy  measurements  for  the 
same  rotational  motion  as  in  Fig.  1.  Noise  signals  were  intro¬ 
duced  using  a  random  number  generator.  It  is  noted  that  the 
switch  flag  flipped  many  times,  and  the  value  of  cos  9  was 
kept  above  e  =  0.1  at  all  times.  The  trajectory  of  the  pitch 
angle  follows  the  same  rise  and  fall  pattern  as  in  Fig.  1,  except 
with  added  noise.  The  roll  and  yaw  angles  flipped  from  0°  to 
180°  numerous  times,  signifying  that  the  pitch  angle  jumped 
above  and  below  90°.  Figs.  1  and  2  show  the  trajectory  of  the 
roll,  pitch,  and  yaw  angles  for  visualization  purposes.  Although 
there  are  jumps  in  roll  and  yaw,  there  are  no  jumps  in  the  trajec¬ 
tory  of  the  estimated  quaternion,  as  shown  in  the  corresponding 
plot  of  the  estimated  quaternion  components  in  Fig.  3. 

F.  Alternative  Method  for  Singularity  Avoidance 

The  aforementioned  procedure  amounts  to  a  second  (  virtual) 
sensor  package  that  is  offset  from  the  basic  (physical)  sen¬ 
sor  package  by  a  rotation  of  —20°  about  the  body  right-side 
coordinate  axis  (y^-axis).  in  this  method,  when  the  physical 
sensor  approaches  an  Euler  angle  singularity  (as  indicated  by 
the  switch  flag),  the  virtual  sensor  data  are  substituted  for 
the  physical  sensor  data,  thereby  avoiding  division  by  a  small 
number  and  attendant  sensitivity  to  noise  in  calculating  the 
desired  orientation  quaternion.  An  alternative  to  this  approach 
is  to  use  a  second  set  of  Euler  angles  such  that  the  second  set 
has  its  singularities  located  in  a  different  spatial  orientation  than 
the  (f>,  9 ,  and  if)  aerospace  sets  [18]  that  are  used  so  far  in 
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Fig.  2.  Roll,  pitch,  and  yaw  angles;  singularity  condition;  and  switch  flag  during  a  180°  rotation  in  pitch  axis  with  noisy  simulated  data.  The  parameters  used 
are  e  =  0.1  and  the  offset  angle  a  =  20°. 


Components  of  estimated  quaternion  from  factored  quaternion  algorithm 
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Fig.  3.  Components  of  the  estimated  quaternion  during  a  180°  rotation  about  the  pitch  axis  with  noisy  simulated  data. 


this  paper  and  to  switch  between  these  sets  as  needed  to 
avoid  singularities.  This  possibility  is  explored  in  the  following 
discussion. 

Per  earlier  discussion,  one  interpretation  of  the  aerospace 
Euler  angles  [18]  is  that,  starting  with  body  axes  (nose,  right 
side,  and  belly)  aligned  with  Earth  axes  (North,  East,  and 


down),  if)  is  the  yaw  angle  associated  with  rotation  of  an  object 
about  its  belly  axis.  This  is  followed  by  a  pitch  rotation  9  about 
the  (rotated)  right-side  axis  and,  finally,  a  roll  rotation  0  about 
the  (rotated)  nose  axis.  It  should  be  noted  that  the  aforemen¬ 
tioned  reserved  names  for  these  angles  are  alternatively  referred 
to,  respectively,  as  azimuth,  elevation ,  and  bank  angles  [18]. 
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Fig.  4.  Orientation  estimate  produced  by  the  QUEST  algorithm  and  FQA  with  a  periodic  90°  rotation  about  the  roll  axis  using  accelerometer  and  magne¬ 
tometer  data. 


The  former  names  are  frequently  used  when  thinking  about 
rotations  viewed  from  the  object  (for  example,  as  by  a  pilot), 
while  the  latter  names  are  more  commonly  used  by  observers 
situated  in  an  Earth-fixed  coordinate  system  (for  example,  as  in 
held  artillery). 

An  instance  of  an  alternate  set  of  Euler  angles  is  provided  by 
the  angles  resulting  from  exchanging  the  temporal  order  of  the 
first  two  rotations  described  earlier.  If  this  is  done,  due  to  the 
well-known  noncommutativity  of  finite  rotations,  the  resulting 
angles  will  have  different  values  as  compared  to  the  aerospace 
set.  Furthermore,  the  singularities  of  the  alternate  set  will  occur 
in  a  different  orientation.  To  see  that  this  is  so,  consider  an 
object  initially  aligned  in  the  reference  orientation  described 
earlier.  If  this  object  is  subjected  first  to  a  90°  rotation  about 
its  right-side  axis  (j/b- axis),  its  belly  axis  (zf,-axis)  will  then 
point  North.  A  subsequent  rotation  of  90°  about  this  (rotated) 
belly  axis  will  cause  the  nose  axis  of  the  object  to  point  East, 
corresponding  to  the  axis  of  the  first  rotation.  Further  reflection 
on  this  analysis  shows  that,  for  any  initial  right-side  (East)  axis 
rotation,  if  this  is  followed  by  a  ±90°  belly  axis  rotation,  the 
object  nose  axis  will  point  either  East  or  West;  in  either  case, 
it  will  be  parallel  to  the  initial  rotation  axis.  These  are,  by 
definition,  singular  conditions,  which  are  sometimes  called  the 
gimbal  lock.  On  the  other  hand,  from  (30)  and  (31),  for  the 
standard  aerospace  Euler  angles,  singularities  exist  if  and  only 
if  the  object  nose  axis  points  straight  up  or  down.  This  means 
that  singularities  in  the  formulas  used  for  determination  of  the 
orientation  quaternion  can  be  avoided  by  switching  between 
these  two  Euler  angle  sets  as  needed. 

Evidently,  the  use  of  alternative  Euler  angles  requires  the 
derivation  of  new  formulas  for  the  computation  of  the  cor¬ 
responding  orientation  quaternions  as  well  as  expressions  for 


Fig.  5.  Block  diagram  of  the  QUEST  algorithm/FQA  with  a  low-pass  filter. 

switching  from  one  Euler  set  to  the  other  when  a  singular 
condition  is  approached.  While  this  is  straightforward  and 
certainly  practical,  it  is  believed  that  the  use  of  an  offset 
virtual  sensor,  as  in  the  implementation  in  this  paper,  is  both 
conceptually  and  mathematically  simpler  and  leads  to  simpler 
computer  software.  Therefore,  all  of  the  following  experimental 
results  were  obtained  using  the  virtual  offset  sensor  switching 
approach. 

III.  Experimental  Results 

Sensor  data  for  the  experiments  were  collected  using  a 
MARG  III  inertial/magnetic  sensor  module,  which  was  de¬ 
signed  by  the  authors  and  fabricated  by  McKinney  Technol¬ 
ogy  [16].  Primary  sensing  components  for  this  unit  include  a 
pair  of  two-axes  Analog  Devices  ADXL202E  micromachined 
accelerometers,  and  Honeywell  HMC1051Z  and  HMC1052 
one-  and  two-axes  magnetometers.  Overall,  dimensions  of  the 
MARG  III  unit  are  approximately  0.7  in  x  1.2  in  x  1.0  in. 
Although  the  MARG  III  units  contain  angular  rate  sensors,  no 
rate  data  were  used  in  the  experiments  described  in  this  paper. 

A.  Testing  of  Static  and  Dynamic  Accuracy 

Controlled  rotations  of  the  sensor  modules  were  performed 
by  placing  an  inertial/magnetic  sensor  module  on  a  HAAS 
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Fig.  6.  Orientation  estimate  produced  by  the  QUEST  algorithm  and  FQA  with  a  90°  rotation  in  roll  axis  using  low-pass  filtered  accelerometer  data. 


precision  tilt  table.  The  table  has  two  degrees  of  freedom  and 
is  capable  of  positioning  with  an  accuracy  of  0.001°  at  rates 
ranging  from  0.001  to  80°/s.  In  order  to  mitigate  any  possible 
magnetic  effects  generated  by  the  steel  construction  of  the  tilt 
table,  the  sensor  unit  was  mounted  on  a  nonferrous  extension 
above  the  table.  In  each  of  these  experiments,  the  sensor 
module  was  initially  placed  with  its  xsyszs- axes,  respectively, 
aligned  with  the  NED  directions.  Following  an  initial  still 
period,  the  sensor  module  was  then  subjected  to  a  series  of 
rotations. 

Fig.  4  shows  the  performance  of  each  of  the  two  algorithms 
using  raw  accelerometer  and  magnetometer  data.  The  sensor 
was  rotated  —90°  about  the  a;-axis  at  a  rate  of  60°/s  and  then 
rotated  90°  at  the  same  rate  (in  the  reverse  direction)  for  two 
cycles.  The  plots  to  the  left  show  the  orientation  estimated 
by  the  QUEST  algorithm,  and  the  graphs  to  the  right  show 
the  orientation  estimated  by  the  FQA.  The  small  pitch  and 
yaw  motions  seen  in  the  pitch  and  yaw  subplots  are  due  to 
misalignment  of  the  sensor  module  with  the  motion  axes  of 
the  tilt  table.  It  can  be  seen  that  both  algorithms  were  able 
to  correctly  estimate  the  roll  angle  before  the  first  (negative) 
rotation,  between  the  first  and  second  (positive)  rotations,  and 
after  the  second  rotation.  Neither  was  able  to  correctly  estimate 
orientation  during  rotational  motion.  Similar  results  were  ob¬ 
served  in  experiments  involving  different  angles  of  rotation  at 
different  rates. 

During  motion,  the  accelerometers  measure  the  sum  of 
gravity  and  motion-induced  acceleration.  In  the  case  of  the 
experiments  described  here,  the  motion-induced  acceleration  is 
due  to  the  motion  of  the  tilt  table  and  flexing  of  the  nonferrous 
extension  on  which  the  sensor  module  was  mounted.  Since  both 
the  QUEST  algorithm  and  FQA  are  single-frame  algorithms, 


neither  is  able  to  filter  out  transient  nongravitational  accelera¬ 
tions  that  occur  during  motion. 

Fig.  5  shows  a  revised  approach  in  which  a  low-pass  filter  for 
accelerometer  data  is  combined  with  the  FQA  or  QUEST  algo¬ 
rithm.  To  examine  the  performance  of  the  QUEST  algorithm 
and  FQA  in  conjunction  with  a  low-pass  filter,  the  rotation  ex¬ 
periments  were  repeated.  Fig.  6  shows  the  performance  during 
90°  rolls  at  a  rate  of  60° /s.  A  comparison  of  Fig.  6  with  Fig.  4, 
in  which  the  sensor  module  was  subjected  to  the  same  rotations, 
shows  that  either  algorithm  can  be  used  to  track  the  orientation 
of  a  rigid  body  in  a  dynamic  environment  when  acceleration 
data  are  low-pass  filtered.  Again,  similar  results  were  observed 
in  experiments  involving  different  angles  of  rotation  at  different 
rates. 

B.  Avoidance  of  Singularity  Conditions 

Within  the  FQA,  three  half-angles  are  used  to  derive  an 
orientation  quaternion.  Measurement  vectors  are  rotated  by 
an  angle  a  when  the  pitch  angle  approaches  ±90°  and  cos  9 
approaches  zero.  Figs.  7  and  8,  respectively,  show  the  operation 
of  the  FQA  and  its  output  during  110°  pitch  rotations.  During 
this  experiment,  a  was  set  to  45°,  and  e  was  0.2.  The  bottom 
two  subplots  of  Fig.  7  trace  the  value  of  cos  9  and  the  value  of 
the  switch  flag  that  triggers  the  singularity  avoidance  method 
and  show  the  direct  correspondence  between  the  two  in  time.  It 
can  be  observed  that,  each  time  cos  9  was  about  to  become  less 
than  e,  the  switch  flag  was  set  to  one.  The  top  three  subplots  in 
Fig.  7  show  the  angles  calculated  from  the  quaternion  estimate 
produced  by  the  algorithm.  The  apparent  rise  of  the  pitch  angle 
to  90°  and  then  drop  to  70°  is  a  visualization  artifact  due  to 
the  use  of  the  three  angles  for  plotting  purposes.  A  110°  pitch 
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Fig.  7.  Angles,  singularity  condition,  and  switch  flag  of  the  FQA  during  1 10°  rotation  in  pitch  axis. 
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Fig.  8.  Components  of  the  estimated  quaternion  produced  by  the  FQA  during  1 10°  rotation  in  pitch  axis. 


is  represented  as  70°  pitch,  together  with  a  180°  roll  and  a 
±180°  yaw.  At  times,  the  yaw  angle  flips  between  two  alternate 
representations  of  the  same  rotation,  namely,  —180°  and  180°. 
The  roll  angle  is  stable  at  either  0°  or  180°. 

The  quaternion  elements  that  are  shown  in  Fig.  8  are 
smooth  and  exhibit  no  flipping  of  orientation  representations 


or  singularity  artifacts.  The  real  part  of  the  quaternion 
begins  at  1.0  and  changes  to  cos(110°/2)  =  0.5736  during  the 
1 10°  rotations.  The  element  of  the  unit  quaternion  associated 
with  rotations  about  the  pitch  axis  <72  begins  at  zero  and 
changes  to  sin(110°/2)  =0.8192  during  the  110°  rotations. 
The  small  changes  in  q3  and  q3  are  due  to  misalignment 
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Magnetometer  Readings 


Fig.  9.  Components  of  the  normalized  local  magnetic  field  measurement  vector  under  the  influence  of  a  moving  magnetic  field  distortion. 
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Fig.  10.  Orientation  estimate  produced  by  the  QUEST  algorithm  and  FQA  with  a  static  sensor  module  under  the  influence  of  a  moving  magnetic  field  distortion. 


between  the  sensor  module  and  the  motion  axes  of  the  tilt 
table. 

C.  Testing  of  Static  Accuracy  When  Subjected  to  Magnetic 
Field  Variations 

To  test  the  decoupling  property  of  the  FQA,  an  inertial/ 
magnetic  sensor  module  was  mounted  on  a  level  nonferrous 


stationary  platform.  The  sensor  module  xsyszs- axes  were, 
respectively,  aligned  with  the  NED  directions.  Following  an 
initialization  period,  the  sensor  module  was  exposed  to  a 
ferrous  object.  Movement  of  the  ferrous  object  caused  the 
direction  of  the  measured  magnetic  field  to  rotate  by  as  much 
as  360°.  Changes  in  the  measured  magnetic  field  were  ob¬ 
served  in  all  measurement  axes,  as  shown  in  Fig.  9.  Fig.  10 
shows  orientations  calculated  using  the  QUEST  algorithm  and 
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FQA.  It  can  be  observed  that  the  orientations  calculated  using 
the  QUEST  algorithm  (depicted  by  three  subplots  to  the  left) 
exhibited  errors  on  all  axes.  On  the  other  hand,  the  FQA 
(depicted  by  three  subplots  to  the  right)  showed  no  errors  in 
either  the  pitch  or  roll  axes. 

D.  Algorithm  Efficiency 

To  make  a  rough  comparison  of  the  efficiency  of  the  QUEST 
algorithm  and  FQA,  the  time  required  for  each  to  complete 
the  computation  of  5000  orientation  quaternions  was  deter¬ 
mined  using  MATLAB  on  a  PC  with  an  866-MHz  Pentium  III 
processor  and  256-MB  RAM.  This  number  represents  50  s 
of  data  at  a  sampling  rate  of  100  Hz.  Both  algorithms  were 
able  to  complete  the  5000  quaternion  calculations  in  less 
than  10  s.  The  calculations  were  completed  in  9.8  s  by  the 
QUEST  algorithm  and  7.8  s  by  the  FQA.  In  this  experiment, 
the  FQA  was  approximately  25%  faster  than  the  QUEST 
algorithm. 

IV.  Conclusion 

This  paper  presents  a  physically  intuitive  algorithm  for 
calculating  orientation  using  accelerometer  and  magnetome¬ 
ter  data  for  static  or  slow-moving  objects.  The  algorithm 
produces  estimates  in  quaternion  form  through  a  series  of 
sequential  rotations.  These  rotations  can  individually  be  ex¬ 
amined  in  a  manner  similar  to  pitch,  roll,  and  yaw  angles 
in  a  Euler  angle  sequence.  However,  unlike  Euler  angles, 
the  algorithm  incorporates  a  singularity  avoidance  method.  In 
the  algorithm,  magnetometer  data  are  not  used  to  calculate 
orientation  that  is  relative  to  the  vertical  axis;  thus,  mag¬ 
netic  variations  result  in  errors  only  in  the  horizontal  plane. 
This  property  of  the  algorithm  is  experimentally  demonstrated. 
Singularities  in  the  numerical  implementation  are  avoided 
through  the  use  of  a  method  that  assigns  an  offset  body 
coordinate  system  when  a  singularity  occurs.  The  algorithm 
is  efficient  and  does  not  require  the  evaluation  of  trigo¬ 
nometric  functions.  Experimental  results  indicate  that,  when 
combined  with  a  low-pass  filter  for  accelerometer  data,  the 
algorithm  is  able  to  track  orientation.  The  algorithm  has  been 
successfully  used  in  real-time  human-body-motion-tracking 
applications. 

As  a  final  remark,  it  should  be  noted  that  dynamic  estimation 
of  an  orientation  quaternion  through  integration  of  values  for  q 
that  are  obtained  from  angular  rate  sensors  is  equally  likely  to 
produce  a  quaternion  with  a  negative  real  part  as  a  positive  real 
part.  On  the  other  hand,  the  FQA  always  produces  a  quater¬ 
nion  with  a  positive  real  part.  Since  both  q  and  —  q  produce 
identical  rotations,  it  is  important  to  use  both  of  these  values  in 
computing  A q,  which  is  the  difference  between  the  predicted 
and  measured  value  for  q  that  is  used  in  drift  correction,  and 
to  then  choose  the  value  for  A  q  which  is  smaller  in  absolute 
value.  This  comparison  is  not  necessary  in  some  previous  (less 
efficient  and  less  accurate)  approaches  to  drift  correction  [21] 
since  these  methods  usually  directly  incorporate  old  estimates 
of  q  into  estimation  of  A q,  while  this  is  not  the  case  for  the 
FQA  estimate. 
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